#include <iostream>
#include <cmath>
#include <vector>
#include <numeric>

#ifndef EKF_FUSION_H
#define EKF_FUSION_H

    using namespace std; // 使用命名空间std

    namespace faf // filter add fusion namespace
    {
        typedef struct
        {
            // 平方差和
            float sum_square = 0.0;
            // 均值
            float mean = 0.0;
            // 方差
            float variance = 0.0;
            // 标准差
            float std_dev = 0.0;
            // 预测值
            float predicting = 0.0;
            // 残差 实际值与估计值之间的差
            float residual = 0.0;
            // 拟合优度 即线性度 约靠近1 线性程度越好
            float r_squared = 0.0;
        } data_stauts_t; // 数据状态

        class fusion
        {
        private:
#define BUFFER_SIZE 3

            float a_buffer[BUFFER_SIZE], b_buffer[BUFFER_SIZE];
            float *a_ptr = a_buffer; //
            float *b_ptr = b_buffer; //

            float output = 0.0; // 输出值

            data_stauts_t data_stauts_a, data_stauts_b; // 数据状态

            float processNoise;     // 过程噪声
            float measurementNoise; // 测量噪声
            float estimatedError;   // 估计误差
            float currentEstimate;  // 当前估计值
            /***
             * 写入数据到缓冲区
             */
            int write_a(float a)
            {
                if (a_ptr == a_buffer + BUFFER_SIZE) // 如果指针指向a_buffer的末尾，则重新指向a_buffer的起始位置
                {
                    a_ptr = a_buffer;
                }
                *a_ptr = a; // 写入数据
                a_ptr++;    // 指针指向下一个位置
                return 0;
            }
            /**
             * 写入数据到缓冲区
             */
            int write_b(float b)
            {
                if (b_ptr == b_buffer + BUFFER_SIZE) // 如果指针指向b_buffer的末尾，则重新指向b_buffer的起始位置
                {
                    b_ptr = b_buffer;
                }
                *b_ptr = b; // 写入数据
                b_ptr++;    // 指针指向下一个位置
                return 0;
            }

            void read_a(float *a)
            {
                for (size_t i = 0; i < BUFFER_SIZE; i++)
                {
                    if (a_ptr + i >= a_buffer + BUFFER_SIZE)
                    {
                        *(a + i) = *(a_ptr + i - BUFFER_SIZE); // 如果指针指向a_buffer的末尾，则重新指向a_buffer的起始位置
                    }
                    else
                    {
                        *(a + i) = *(a_ptr + i); // 读取数据
                    }
                }
            }

            void read_b(float *b)
            {
                for (size_t i = 0; i < BUFFER_SIZE; i++)
                {
                    if (b_ptr + i >= b_buffer + BUFFER_SIZE)
                    {
                        *(b + i) = *(b_ptr + i - BUFFER_SIZE); // 如果指针指向b_buffer的末尾，则重新指向b_buffer的起始位置
                    }
                    else
                    {
                        *(b + i) = *(b_ptr + i); // 读取数据
                    }
                }
            }

            /**
             * 计算数据状态 以及线性化
             * @param data 数据点
             * @param len 数据点数量
             * @param data_stauts 数据状态结构体
             * @return 执行结果
             *          */
            int calculateLinearity(const float *data, int len, data_stauts_t *data_stauts)
            {
                int n = len; // 数据长度
                float sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_x2 = 0.0;
                float slope;     // 斜率
                float intercept; // 截距
                float mean_y;    // 均值

                // 计算必要的和
                for (int i = 0; i < n; ++i)
                {
                    sum_x += i;            // x 为序列索引
                    sum_y += data[i];      // y 为数据值 数据和
                    sum_xy += i * data[i]; // xy 为数据索引与数据值的乘积
                    sum_x2 += i * i;       // x^2 为数据索引的平方
                }

                // 计算斜率（slope）和截距（intercept） 均值 (mean_y)
                slope = (n * sum_xy - sum_x * sum_y) / (n * sum_x2 - sum_x * sum_x);
                intercept = (sum_y - slope * sum_x) / n;
                mean_y = sum_y / n;
                data_stauts->mean = mean_y;                                // 赋值均值
                data_stauts->predicting = slope * BUFFER_SIZE + intercept; // 计算预测值
                // 计算总平方和（Total Sum of Squares）
                for (int i = 0; i < n; ++i)
                {
                    data_stauts->sum_square += (data[i] - mean_y) * (data[i] - mean_y); // 计算总平方差和(单个数据-均值)^2
                }

                data_stauts->variance = data_stauts->sum_square / n; // 计算方差

                // 计算残差平方和（Residual Sum of Squares）
                for (int i = 0; i < n; ++i)
                {
                    float predicted = slope * i + intercept; // 计算 n_i的预测值
                    data_stauts->residual += (data[i] - predicted) * (data[i] - predicted);
                }
                // 拟合优度 计算R²值 值越大，线性度越好 最大值为1
                data_stauts->r_squared = 1 - (data_stauts->residual / data_stauts->sum_square);
                return 0;
            }
            // 公共类
        public:
            /**
             * @brief 构造函数 创建时被调用
             * @param processNoise_t 过程噪声
             * @param measurementNoise_t 测量噪声
             * @param estimatedError_t 估计误差 (初始值一般为0)
             */
            fusion(float processNoise_t, float measurementNoise_t, float estimatedError_t)
            {
                processNoise = processNoise_t;         // 过程噪声
                measurementNoise = measurementNoise_t; // 测量噪声
                estimatedError = estimatedError_t;     // 估计误差
            }

            float EKF_Fusion(float a, float b);
        };


} // namespace fusion
#endif // EKF_FUSION_H